#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
#pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
#include <Eigen/Dense>
#pragma GCC diagnostic pop
#include <buola/buola.h>
#include <buola/mat.h>

using namespace buola;

int main(int /*argc*/,char **argv)
{
/*
    Eigen::MatrixXd a=Eigen::MatrixXd::Random(5,7);
    msg_info() << a << "\n";
    auto lSVD=a.jacobiSvd(Eigen::ComputeThinU|Eigen::ComputeThinV);
    Eigen::MatrixXd b=lSVD.matrixV()*lSVD.singularValues().asDiagonal()*lSVD.matrixU().transpose();
    msg_info() << b << "\n";
    Eigen::MatrixXd c=a*b;
    msg_info() << c << "\n";
    Eigen::MatrixXd d=b*a;
    msg_info() << d << "\n";
    Eigen::MatrixXd e=a*b*a;
    msg_info() << e << "\n";
*/    
/*
    int lSize=atoi(argv[1]);
    msg_info() << lSize << "\n";
    Eigen::MatrixXd a(lSize,lSize);
    Eigen::MatrixXd b(lSize,lSize);
    Eigen::MatrixXd c(lSize,lSize);
    Eigen::MatrixXd d(lSize,lSize);
    start_timer();
    Eigen::MatrixXd e=a*b*c*d;
    end_timer();
*/    
/*    
    start_timer();
    Eigen::MatrixXd a=Eigen::MatrixXd::Random(lSize,lSize);
    Eigen::MatrixXd b=Eigen::MatrixXd::Random(lSize,lSize);
    Eigen::MatrixXd c=Eigen::MatrixXd::Random(lSize,lSize);
    Eigen::MatrixXd d=Eigen::MatrixXd::Random(lSize,lSize);
    Eigen::MatrixXd e,f,g;
    end_timer();

    start_timer();
    e=a+b+c+d;
    end_timer();
    start_timer();
    e=a.transpose()+b.transpose()+c.transpose()+d.transpose();
    end_timer();
    start_timer();
    e.transpose()=a.transpose()+b.transpose()+c.transpose()+d.transpose();
    end_timer();
    start_timer();
    e.transpose()=a+b+c+d;
    end_timer();
    start_timer();
    e=a.transpose()+b+c.transpose()+d.transpose();
    end_timer();
    start_timer();
    e=a+b+c+d;
    end_timer();

    msg_info() << "aaa\n";
    start_timer();
*/
/*    end_timer();
    double *d2=new double[lSize*lSize];
    double *e2=new double[lSize*lSize];
    end_timer();
    start_timer();
    for(int i=0;i<lSize;i++)
    {
        for(int j=0;j<lSize;j++)
        {
            a2[i*lSize+j]=a(i,j);
            b2[i*lSize+j]=b(i,j);
            c2[i*lSize+j]=c(i,j);
            d2[i*lSize+j]=d(i,j);
        }
    }
    end_timer();
    */
/*    start_timer();
    for(int i=0;i<lSize*lSize;i+=4)
    {
        __m128 r=_mm_loadu_ps(&a2[i]);
        __m128 o=_mm_loadu_ps(&b2[i]);
        r=_mm_add_ps(r,o);
        o=_mm_loadu_ps(&c2[i]);
        r=_mm_add_ps(r,o);
        o=_mm_loadu_ps(&d2[i]);
        r=_mm_add_ps(r,o);
        _mm_storeu_ps(&e2[i],r);
    }
    end_timer();
    start_timer();
    for(int i=0;i<lSize*lSize;i+=4)
    {
        __m128 r=_mm_loadu_ps(&a2[i]);
        __m128 o=_mm_loadu_ps(&b2[i]);
        r=_mm_add_ps(r,o);
        o=_mm_loadu_ps(&c2[i]);
        r=_mm_add_ps(r,o);
        o=_mm_loadu_ps(&d2[i]);
        r=_mm_add_ps(r,o);
        _mm_storeu_ps(&e2[i],r);
    }
    end_timer();
*/
/*
    double *a2=new double[lSize*lSize];
    double *b2=new double[lSize*lSize];
    start_timer();
    for(int j=0;j<10000;j++)
    {
        int lD=lSize*lSize;
    double *c2=new double[lD];
    for(int i=0;i<lD;i+=2)
    {
        __m128d r=_mm_loadu_pd(&a2[i]);
        __m128d o=_mm_loadu_pd(&b2[i]);
        r+=o;
        _mm_store_pd(&c2[i],r);
    }
    delete[] c2;
    }
    end_timer();
*/    
/*
    start_timer();
    double *c2=new double[lSize*lSize];
    for(int i=0;i<lSize;i++)
    {
        for(int j=0;j<lSize;j++)
            c2[i*lSize+j]=a2[i*lSize+j]+b2[i*lSize+j];
    }
    end_timer();
*/
/*    for(int i=0;i<lSize*lSize;i++)
    {
        c2[i]=a2[i]+b2[i];
    }
    end_timer();
    start_timer();
    for(int i=0;i<lSize;i++)
    {
        for(int j=0;j<lSize;j++)
            c2[j*lSize+i]=a2[j*lSize+i]+b2[j*lSize+i];
    }
    end_timer();
    start_timer();

    msg_info() << a.cwiseProduct(b) << "\n";
    msg_info() << (a+b).cwiseProduct(-c*d) << "\n";

    start_timer();
    for(int i=0;i<lSize000;i++)
    {
        e=(a+b).cwiseProduct(-c*d);
    }
    end_timer();
*/
    int K=85;
    int N=250;
    Eigen::MatrixXd pZgivenC=Eigen::MatrixXd::Random(K+1,N);

    start_timer();
    //iteratively normalize rows and columns. columns are normalized to one and rows are normalized to the visibility term.
    for (int i=0; i<5; i++) 
    {
        //normalize rows
        //FIXME does the k+1 row need to be normalized?
        for (int k=0; k<K; k++) {
                pZgivenC.row(k) =pZgivenC.row(k) * (1.0/ pZgivenC.row(k).sum());
        }
        pZgivenC.row(K) = pZgivenC.row(K) / pZgivenC.row(K).sum();

        //normalize cols
//        pZgivenC = pZgivenC * ((Eigen::VectorXd) pZgivenC.colwise().sum().array().inverse()).asDiagonal();
    }
    end_timer();
    msg_info() << pZgivenC.sum() << "\n";
    
    buola::mat::CMat_d mPZGivenC=mat::random(K+1,N);
    start_timer();
    for (int i=0;i<5;i++) 
    {
        //normalize rows
        //FIXME does the k+1 row need to be normalized?
        for(int k=0;k<K;k++) 
        {
             mPZGivenC(k,nAll)=mPZGivenC(k,nAll)*(1.0/sum(mPZGivenC(k,nAll))); //fix mat so that it's easier
        }
        mPZGivenC(K,nAll)=mPZGivenC(K,nAll)*(1.0/sum(mPZGivenC(K,nAll)));
  
        //normalize cols
//        mPZGivenC=mPZGivenC**extend(1.0/sum(cols(mPZGivenC)));
    }
    end_timer();
    msg_info() << sum(mPZGivenC) << "\n";
}

